// PX4CtrlFSM.h

#ifndef PX4CtrlFSM_H
#define PX4CtrlFSM_H


#include <ros/ros.h>
#include <geometry_msgs/PoseStamped.h>
#include <nav_msgs/Path.h>
#include <visualization_msgs/MarkerArray.h>
#include <mavros_msgs/State.h>
#include <geometry_msgs/TwistStamped.h>
#include <string>
#include "Eigen/Eigen"
#include "math.h"
#include <boost/filesystem.hpp>
#include <mavros_msgs/RCIn.h>
#include <mavros_msgs/SetMode.h>
#include "offboard_a2VISR/PosVelAcc.h"
#include "offboard_a2VISR/commonUsage.h"
#include "offboard_a2VISR/pid.h"
#include "offboard_a2VISR/rosRelated.h"
#include "offboard_a2VISR/DebugPid.h"
#include "std_msgs/Float64MultiArray.h"

using namespace Eigen;
using namespace std;

#define LOOPRATE 25
#define GRAVITATIONAL_ACC 9.794 // TODO： The gravity of Shanghai

class PX4CtrlFSM{
    private:
        // const int LOOPRATE = 25;
        ///========= 参数文件 =========///
        float rosParamBaseThrust;                                // 飞机基础油门值。各架飞机不同
        vector<float> rosParamThrustLimit = vector<float>(2, 0); // 油门上下限
        // PID 控制参数
        vector<float> rosParamPosKP = vector<float>(3, 0);
        vector<float> rosParamPosKI = vector<float>(3, 0);
        vector<float> rosParamPosKD = vector<float>(3, 0);
        vector<float> rosParamVelKP = vector<float>(3, 0);
        vector<float> rosParamVelKI = vector<float>(3, 0);
        vector<float> rosParamVelKD = vector<float>(3, 0);

        vector<float> rcParamXfeedLimit = vector<float>(2, 0);
        vector<float> rcParamYfeedLimit = vector<float>(2, 0);
        vector<float> rcParamZfeedLimit = vector<float>(2, 0);

        ros::Publisher pubPID;
        ros::Publisher pubPx4Attitude;
        ros::Publisher pubDroneTargetEulerThrust;
        ros::Publisher pubTargetPose;
        ros::Publisher pubRCfeed;

    public:
        // define the rc signal
        struct RC_Data_t {
            bool landing;
            bool hover;
            bool takeoff;
            bool docking;
        };
        RC_Data_t rc_data = {false, false, false, false};

        int rc_in_feedback = 0;
        int rc_in_feedback_dock = 0;
        // 上一次的拨动状态，初始化为 false 表示未触发 dock 模式
        bool was_docking_activated = false;

        // Define the flight mode enum
        enum FlightMode { IDLE, TAKEOFF, HOVER, LANDING, DOCKING };
        FlightMode flight_mode = IDLE;
        // Function to convert FlightMode enum to string
        std::string flightModeToString(FlightMode mode) {
            switch(mode) {
                case IDLE: return "IDLE";
                case TAKEOFF: return "TAKEOFF";
                case HOVER: return "HOVER";
                case LANDING: return "LANDING";
                case DOCKING: return "DOCKING";
                default: return "UNKNOWN";
            }
        }

        mavros_msgs::State uav_cur_state;             
        geometry_msgs::PoseStamped uav_cur_pose;
        nav_msgs::Odometry uav_cur_poseOdom;
        std_msgs::Float64MultiArray rc_output_msg;
        geometry_msgs::PoseStamped uav_cur_euler;
        geometry_msgs::PoseStamped offb_dockini_ori;
        geometry_msgs::PoseStamped orientationDocking;

        ///=========初始化YAW =========///
        double targetPoseLastTimeStamp = 0.0; // 上一次目标点时间戳
        double yaw_last = 0.0;
        int count_yaw_init;
        double yaw_init;
        double yaw_thre;

        ///========= joystick =========///
        int roll_channel = 0; // 横滚通道
        int pitch_channel = 1; // 俯仰通道
        int throttle_channel = 2; // 油门通道
        int yaw_channel = 3; // 偏航通道
        int mode_channel = 5; // mode通道
        int dock_channel = 7; // dock通道

        int rc_in_roll_feedback = 0;
        int rc_in_pitch_feedback = 0;
        int rc_in_thro_feedback = 0;
        int rc_in_yaw_feedback = 0;
        int rc_in_thro_feedback_last = 0;

        // 定义 RC 通道信号
        struct RcSignal {
            int min;
            int max;
            int mid;
        };

        // 定义遥控器输入信号范围
        RcSignal rcSignalRange_x = {1094, 1934, 1510};
        RcSignal rcSignalRange_y = {1094, 1934, 1515};
        RcSignal rcSignalRange_z = {1126, 1934, 1554};

        // 定义 RC 参数和范围参数
        struct RcParams {
            float x;
            float y;
            float z;
        };

        struct RcRange {
            float min_xy;
            float max_xy;
            float min_z;
            float max_z;
        };

        struct ModeParams {
            // js 在不同阶段的积分速度
            RcParams rc_rate;
            // js 在不同阶段的区间
            RcRange rc_range;
        };

        // rc_feed 当前的累计值
        RcParams rc_feed;
        // 定义参数
        ModeParams current_params;
        ModeParams hover_params;
        ModeParams landing1_params;
        ModeParams landing2_params;
        ModeParams zero_params = {{0.0, 0.0, 0.0}, {-0.1, 0.1, -0.1, 0.1}};  // 全为0的参数集

        bool landing_process1_flag = false;
        bool init_dock = false;

        ///========= 飞行控制变量 =========///
        float baseThrust;
        PID pidPX, pidPY, pidPZ;
        PID pidVX, pidVY, pidVZ;
        
        geometry_msgs::Vector3 angleTarget;           // 期望欧拉角
        geometry_msgs::Vector3 angleDocking;          // docking欧拉角
        geometry_msgs::Quaternion orientationTarget; // 发给无人机的姿态指令  期望四元数
        double thrustTarget;                         // 期望油门
        double pidZVelLast = 0;                      // 上一次z轴速度环输出的结果
        mavros_msgs::AttitudeTarget msgTargetAttitudeThrust;
        geometry_msgs::PoseStamped msgDroneTargetEulerThrust;
        geometry_msgs::Vector3 droneTargetEuler; // 飞行控制： 期望欧拉角与油门值，仅供自己输出参考

        ///========= 临时变量，用于传入当前状态与目标点 =========///
        Eigen::Vector3d odomCurrPos;
        Eigen::Vector3d odomCurrVel;
        Eigen::Vector3d TargetPos;
        Eigen::Vector3d TargetVel;
        Eigen::Vector3d TargetAcc;

        double TargetYaw;
        double relativeTime;

        //默认构造函数
        PX4CtrlFSM(){};
        // 构造函数
        PX4CtrlFSM(ros::NodeHandle& nh);
        //默认析构函数
        ~PX4CtrlFSM();

        ///========= 类成员函数定义 =========///
        void pose2pva(geometry_msgs::PoseStamped pose, offboard::PosVelAcc &pva);
        void uav_state_cb(const mavros_msgs::State::ConstPtr &msg);
        void uav_pose_cb(const geometry_msgs::PoseStamped::ConstPtr &msg);
        void uav_poseOdom_cb(const nav_msgs::Odometry::ConstPtr &msg);
        void rc_in_cb(const mavros_msgs::RCIn::ConstPtr& msg);
        void loadParameters(ros::NodeHandle& nh);
        void waitForTargetPosition(ros::Rate& rate);
        void initializeYaw(ros::Rate& rate);
        geometry_msgs::Vector3 quaternion2euler(float x, float y, float z, float w);
        geometry_msgs::Vector3 quaternion2Euler_ros(geometry_msgs::Quaternion q);

        geometry_msgs::PoseStamped euler2quaternion(float roll, float pitch, float yaw);
        bool px4AttitudeCtlInitRosParam();
        void assignCurrentPositionVelocityAndYaw(const ros::Time& startTime, const offboard::PosVelAcc& droneTargetPVA);

        void px4AttitudeCtlPVA(double _currTime,
                       Eigen::Vector3d CurrPos,
                       Eigen::Vector3d CurrVel,
                       Eigen::Vector3d TargetPos,
                       Eigen::Vector3d TargetVel,
                       Eigen::Vector3d TargetAcc,
                       double TargetYaw,
                       mavros_msgs::State &state);


};



#endif